
#ifndef SIGN
#define SIGN(x) ((x>0)?1:((x<0)?-1:0))
#endif SIGN
#include<Algorithm/points_cloud_algorithm.h>
#include<Mesh/libcell_iterator.h>
#include<tool/libcell_kd_node_iterator.h>
#include<tool/libcell_octree_iterator.h>
#include<Matrix/svd.h>


#ifdef LIBCELL_USING_CLAPACK
#include<clapack/include/libcell_clapack.h>
#endif
#define quote lib_cell_quote
#ifdef LIBCELL_USING_CLAPACK

static inline double compute_visual_avarage_distance(Node* n)
{
    double* box=mesh_compute_bounding_box_from_node(n);
    double min1=box[3]-box[0],min2=box[4]-box[1],min3=box[5]-box[2];
    double mid=min1>=min2?(min3>=min1?(min1):(min2>=min3?(min2):(min3))):(min3>=min2?(min2):(min1>=min3?(min1):(min3)));
    double vdis=mid/70.0;
    return vdis;
}

double*compute_pca_result_using_clapack(Node* n)
{    
    int len=node_size(n);
    if(len<3)
    {
        return NULL;
    }
    template_v* v=(template_v*)(n->value);
    double *A=(double*)malloc(sizeof(double)*len*v->point_size);

    int i=0;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v=(template_v*)(nit->value); 
        for(int j=0;j<v->point_size;j++)
        {
            A[i+j*len ]=v->point[j];
        }
        i++;
    }
    double *B=(double*)malloc(sizeof(double)*v->point_size); 
    memset(B,0,sizeof(double)*v->point_size);
    for(i=0;i<len;i++)
    {
        for(int j=0;j<v->point_size;j++)
        {
            B[j]+=A[i+j*len] ;
        }
    }
    for(int j=0;j<v->point_size;j++)
    {
        B[j]/=(double)len;
    }
    for(i=0;i<len;i++)
    {
        for(int j=0;j<v->point_size;j++)
        {
            A[i+j*len]-=B[j];
        }
    } 
    free(B);
    double *s=(double*)malloc(sizeof(double)*v->point_size), *VT=(double*)malloc(sizeof(double)*v->point_size*v->point_size), *U=(double*)malloc(sizeof(double)*len*len); 
    clapack_svd(A,len,3,s,U,VT);
    free(A);free(U);free(s); 
    return VT;
}

void points_cloud_compute_normal_using_clapack(Node* n,KD_Node*kdtree,int num)
{
    int i=0;
    double *normal=NULL;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
    
        template_v* v=(template_v*)(nit->value);
        Node* n1=kd_tree_find_n_adjacent_verticesn(v,kdtree,num);
        double* axis=compute_pca_result_using_clapack(n1);  
        normal=(double*)malloc(sizeof(double)*3);
        normal[0]=axis[2];normal[1]=axis[5];normal[2]=axis[8];
        v->prop=normal; 
        free(axis);
        free_node(n1);
        i++;
    } 
} 




int is_boundary_vertex_in_points_cloud(KD_Node*tree,template_v* v,double r)
{
    Node* n=kd_tree_find_nearest_sphere(v,r,tree);
    double* pcas= compute_pca_result_using_clapack(n);
    int * grid=(int*)malloc(sizeof(int)*25);
    memset(grid,0,sizeof(int)*25);
    double * B=(double*)malloc(sizeof(double)*v->point_size*v->point_size);
    memmove(B,pcas,sizeof(double)*v->point_size*v->point_size);
    clapack_inverse(B,v->point_size); 
    double* C=(double*)malloc(sizeof(double)*v->point_size);
    double step=r/5.0;
     
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v1=(template_v*)(nit->value);
        for(int j=0;j<v->point_size;j++)
        {
            C[j]=v1->point[j]-v->point[j];
        }

        double* temp_m=clapack_mult(B,C,v->point_size,1,v->point_size);
        int x=0,y=0;
        x= (int)((fabs(temp_m[0])-step)/2.0*step);
        x<0?(x=0):(x=x+1);x>2?(x=2):(x=x);  
        y= (int)((fabs(temp_m[1])-step)/2.0*step);
        y<0?(y=0):(y=y+1);  y>2?(y=2):(y=y);
        x=SIGN(temp_m[0])*x;y=SIGN(temp_m[1])*y;
        x+=2;y+=2;
        grid[x*5+y]=1;
    }
    int re=0;
    int xs[2][2]={{0,1},{3,4}};
    for(int i=0;i<2;i++)
    {
        for(int j=0;j<2;j++)
        {
            if(grid[xs[i][0]*5+xs[j][0]]==0&& grid[xs[i][0]*5+xs[j][1]]==0 && grid[xs[i][1]*5+xs[j][0]]==0&& grid[xs[i][1]*5+xs[j][1]]==0)
            {
                re=1;
                break;
            }
        }
        if(re==1)
        {
            break;
        }
    }
    free(grid);
    free(pcas);
    free_node(n);
    return re;
}
#endif

double**compute_pca_result(Node* n)
{
    int len=node_size(n);
    if(len<3)
    {
        return NULL;
    }
    template_v* v=(template_v*)(n->value);
    double **A=(double**)malloc(sizeof(double*)*len);
    int i=0;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v=(template_v*)(nit->value); 
        A[i]=(double*)malloc(sizeof(double)*v->point_size);
        for(int j=0;j<v->point_size;j++)
        {
            A[i][j]=v->point[j];
        }
        i++;
    }
    double *B=(double*)malloc(sizeof(double)*v->point_size); 
    memset(B,0,sizeof(double)*v->point_size);
    for(i=0;i<len;i++)
    {
        for(int j=0;j<v->point_size;j++)
        {
            B[j]+=A[i][j] ;
        }
    }
    for(int j=0;j<v->point_size;j++)
    {
        B[j]/=(double)len;
    }
    for(i=0;i<len;i++)
    {
        for(int j=0;j<v->point_size;j++)
        {
            A[i][j]-=B[j];
        }
    } 
    free(B);
    double *s=(double*)malloc(sizeof(double)*v->point_size);

    double **VT=(double**)malloc(sizeof(double*)*v->point_size);
    for(i=0;i<v->point_size;i++)
    {
        VT[i]=(double*)malloc(sizeof(double)*v->point_size);
    } 
    dsvd(A,len,v->point_size,s,VT);

    for(i=0;i<len;i++)
    {
        free(A[i]);
    }
    free(A);free(s); 
    return VT;
}


void points_cloud_compute_normal(Node* n,KD_Node*kdtree,int num)
{
    int i=0;
    double *normal=NULL;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
    
        template_v* v=(template_v*)(nit->value);
        Node* n1=kd_tree_find_n_adjacent_verticesn(v,kdtree,num);
        double** axis=compute_pca_result(n1);  
        normal=(double*)malloc(sizeof(double)*3);
        v->prop=normal; 

        if(axis==NULL)
        {
            normal[0]=0;normal[1]=0;normal[2]=1.0;
        }
        else
        {
            normal[0]=axis[0][0];normal[1]=axis[1][0];normal[2]=axis[2][0];
            for(int j=0;j<v->point_size;j++)
            {
                free(axis[j]);
            }
            free(axis);
        }

       
        free_node(n1);
        i++;
    } 

}



typedef struct Normal_Info{
    double normal[3];
    int is_correct_normal;
}Normal_Info;

static inline void normal_info_init(Normal_Info* ni)
{
    memset(ni->normal,0,sizeof(double)*3);
    ni->is_correct_normal=0;
}
static inline void normal_info_free(Normal_Info*ni)
{
    if(ni==NULL)
    {
        return;
    }
    free(ni);
}




// // @ n:: is vertices
// //@ tree::  map v to open_sets
// // @return::  open sets 

Node* compute_points_open_sets(Node* n,Int_RB_Tree* tree,double r,KD_Node* kdtree)
{
    Node* re=NULL;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        if(tree->find(tree,v->id)==NULL)
        {
            Points_Open_Set* pos=(Points_Open_Set*)malloc(sizeof(Points_Open_Set));
            points_open_set_init(pos);
            pos->v=v;
            pos->n=kd_tree_find_nearest_sphere(v,1.5*r,kdtree);
            re=node_overlying(re,pos);
            for(Node* nit1=pos->n;nit1!=NULL;nit1=(Node*)(nit1->Next))
            {
                template_v* v1=(template_v*)(nit1->value);
                Node* value=(Node*)(tree->find(tree,v1->id));
                if(value==NULL)
                {
                    value=node_overlying(value,pos);
                    tree->insert(tree,v1->id,value);
                }
                else{
                    Node* values_n=node_reverse(value);
                    node_pushback(values_n, pos);
                }
            }           
        }   
        else
        {
            continue;
        }
    }
//**************
//确保最大联通性
//******************
    // for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    // {
    //     template_v* v=(template_v*)(nit->value);

    //     Node*value=(Node*)(tree->find(tree,v->id));
 
    //     if(node_size(value)<2)
    //     {
    //         Node* node_n=kd_tree_find_nearest_sphere(v,1.5*r,kdtree);
    //         if(node_size(node_n)>=2)
    //         {
    //             Points_Open_Set* pos=(Points_Open_Set*)malloc(sizeof(Points_Open_Set));
    //             points_open_set_init(pos);
    //             pos->v=v;
    //             pos->n=node_n;
    //             re=node_overlying(re,pos);
    //             for(Node* nit1=pos->n;nit1!=NULL;nit1=(Node*)(nit1->Next))
    //             {
    //                 template_v* v1=(template_v*)(nit1->value);
    //                 Node* value1=(Node*)(tree->find(tree,v1->id));
    //                 if(value1==NULL)
    //                 {
    //                     value1=node_overlying(value1,pos);
    //                     tree->insert(tree,v1->id,value);
    //                 }
    //                 else{
    //                     Node* values_n=node_reverse(value1);
    //                     node_pushback(values_n, pos);
    //                 }
    //             }
    //         }   
    //         else
    //         {
    //             free_node(node_n);
    //         } 
    //     } 
    // }

    // for(auto it=tree->begin(tree);it.it!=NULL;it++)
    // {
    //     Node*value=(Node*)(it.second);
    //     if(node_size(value)<2)
    //     {


    //     }
    // } 
    //compute adjacents 
    printf(" size1 :%d size2:%d\n",tree->size,node_size(n));
    template_v**values=(template_v**)malloc(sizeof(template_v*)*node_size(re));
    int i=0;
    for(Node* nit=re;nit!=NULL;nit=(Node*)(nit->Next))
    {
        values[i]=((Points_Open_Set*)(nit->value))->v;
        i++;
    }   
    KD_Node* kdtree1= create_kd_tree(values,i,0);   
    for(Node* nit=re;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Points_Open_Set* pos=(Points_Open_Set*)(nit->value);
        Node* n1=kd_tree_find_nearest_sphere(pos->v,3.1*r,kdtree1);

        Node* n2=NULL;
        for(Node* nit1=n1;nit1!=NULL;nit1=(Node*)(nit1->Next))
        {
            template_v* v=(template_v*)(nit1->value);

            Node* n3=(Node*)(tree->find(tree,v->id));
            if(n3->value!=pos){n2=node_overlying(n2,n3->value);}
            // for(Node* nit2=n3;nit2!=NULL;nit2=(Node*)(nit2->Next))
            // {
            //     if(nit2->value!=pos)
            //     {
            //         n2=node_overlying(n2,nit2->value);
            //     }
            // }
        }

        pos->adjacent=n2;
        free_node(n1);
    }   
    free_kdnode(kdtree1);
    free(values);
    return re;
}

static int  adjust_normal_of_one_points_open_set(Points_Open_Set* pos,double* normal)
{
    if(pos->normal!=NULL)
    {
        return 0;
    }
    pos->normal=(double*)malloc(sizeof(double)*3);
    memset(pos->normal,0,sizeof(double)*3);
    for(Node* nit=pos->n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        Normal_Info* ni=(Normal_Info*)(v->prop);
        if(ni->is_correct_normal==0&& inner_product(ni->normal,normal,3)<0)
        {   
            ni->normal[0]=-ni->normal[0];ni->normal[1]=-ni->normal[1];ni->normal[2]=-ni->normal[2];   
        }   
        
        ni->is_correct_normal=1;
        pos->normal[0]+=ni->normal[0];pos->normal[1]+=ni->normal[1];pos->normal[2]+=ni->normal[2];
    }
    
    return 1;
}

static inline double* tools_compute_normal(template_v*v1,template_v* v2,double* dir)
{

    double dir1[3]={v2->point[0]-v1->point[0],v2->point[1]-v1->point[1],v2->point[2]-v1->point[2]};
    if(normalize(dir1,3)==0)
    {
        printf("cuowu\n");      
        return NULL;
    }
    double* re=(double*)malloc(sizeof(double)*3);
    double fac=inner_product(dir,dir1,3);
    re[0]=dir[0]-fac*dir1[0];re[1]=dir[1]-fac*dir1[1];re[2]=dir[2]-fac*dir1[2];             

    return re;
}
// //@n: node of open sets
static void set_normal_vector_of_open_sets(Node*n)
{
    if(n==NULL)
    {
        return ;
    }
    double * normal=NULL,*new_normal=NULL;
    Node* n1=NULL;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Points_Open_Set* pos =(Points_Open_Set*)(nit->value);
        if(pos->normal==NULL)
        {
            n1=node_overlying(n1,pos);
            new_normal=(double*)malloc(sizeof(double)*3);
            n1->traits=new_normal;
            Normal_Info* ni=(Normal_Info*)(pos->v->prop);
            new_normal[0]=ni->normal[0];new_normal[1]=ni->normal[1];new_normal[2]=ni->normal[2];
            break;
        }
    }

    if(n1==NULL)
    {
        return ;
    }
    while(n1!=NULL)
    {       
        Node* temp_n=n1;n1=NULL;
        for(Node* nit=temp_n;nit!=NULL;nit=(Node*)(nit->Next))
        {
            Points_Open_Set* pos=(Points_Open_Set*)(nit->value);    
            normal=(double*)(nit->traits);
            int flag=adjust_normal_of_one_points_open_set(pos,normal);
            if(flag!=0)
            {
                for(Node* nit1=pos->adjacent;nit1!=NULL;nit1=(Node*)(nit1->Next))
                {
                    Points_Open_Set* pos1=(Points_Open_Set*)(nit1->value);
                    if(pos1->normal==NULL)
                    {
                        n1=node_overlying(n1,pos1); 
                        new_normal=tools_compute_normal(pos1->v,pos->v,pos->normal);
                        n1->traits=new_normal;
                    }   
                }       
            }
            free(normal);   
            nit->traits=NULL;   
        }
        free_node(temp_n);
       // printf("size:%d\n",node_size(n1));
    }
    set_normal_vector_of_open_sets(n);
}

// //@n: node of open sets
// //
static void set_normal_vector_of_manifold_points(Node* n)
{
    set_normal_vector_of_open_sets(n);
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Points_Open_Set* pos=(Points_Open_Set*)(nit->value);
        if(pos->normal==NULL)
        {
            continue;
        }   
        for(Node* nit1=pos->n;nit1!=NULL;nit1=(Node*)(nit1->Next))
        {
            template_v* v=(template_v*)(nit1->value);
            Normal_Info* ni=(Normal_Info*)(v->prop);
            if(inner_product(ni->normal,pos->normal,3)<0)
            {
                ni->normal[0]=-ni->normal[0]; ni->normal[1]=-ni->normal[1];ni->normal[2]=-ni->normal[2]; 
            }       
        }   
    } 
} 
void adjust_normal_of_node_vertices(Node*n,double r,KD_Node*kdtree)
{
    Int_RB_Tree* tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree);

    Node* node_prop1=NULL,*node_prop2=NULL;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        node_prop1=node_pushback(node_prop1,v->prop);
        double* normal=(double*)(v->prop);
        Normal_Info* ni=(Normal_Info*)malloc(sizeof(Normal_Info));
        normal_info_init(ni);
        v->prop=ni;
        ni->normal[0]=normal[0];ni->normal[1]=normal[1];ni->normal[2]=normal[2];
        node_prop2=node_pushback(node_prop2,ni);
    }

    Node* n1=compute_points_open_sets(n,tree,r,kdtree);
    int_rb_tree_free_value(tree); 
    int_rb_tree_free(tree); 
    set_normal_vector_of_manifold_points(n1);

    node_prop1=node_reverse(node_prop1);node_prop2=node_reverse(node_prop2);
    Node* nit1=node_prop1;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        Normal_Info* ni=(Normal_Info*)(v->prop);
        double* normal=(double*)(nit1->value);
        memmove(normal,ni->normal,sizeof(double)*3);
        free(v->prop);
        v->prop=nit1->value;
        nit1=(Node*)(nit1->Next);
    } 

    free_node(node_prop1);free_node(node_prop2);
    for(Node* nit=n1;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Points_Open_Set* pos=(Points_Open_Set*)(nit->value);
        points_open_set_free(pos);
    }
    free_node(n1);
}

static inline OC_Node * get_oc_node_according_to_size(Node* n,double len)
{
    OC_Node* ocn = (OC_Node*)malloc(sizeof(OC_Node));
    oc_node_init(ocn);
    template_v* v = (template_v*)(n->value);

    ocn->loc_min[0] = v->point[0];  ocn->loc_min[1] = v->point[1]; ocn->loc_min[2] = v->point[2];
    ocn->loc_max[0] = v->point[0];  ocn->loc_max[1] = v->point[1]; ocn->loc_max[2] = v->point[2];

    for (Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v = (template_v*)(nit->value);
        ocn->value = node_overlying(ocn->value, v);
        for (int i = 0; i < 3; i++)
        {
            v->point[i] > ocn->loc_max[i] ? (ocn->loc_max[i] = v->point[i]) : (v->point[i] < ocn->loc_min[i] ? (ocn->loc_min[i] = v->point[i]) : (i = i));
        }
    }
    double bound[3] = { ocn->loc_max[0] - ocn->loc_min[0],ocn->loc_max[1] - ocn->loc_min[1],ocn->loc_max[2] - ocn->loc_min[2] };
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 2; j++)
        {
            if (bound[j] < bound[j + 1])
            {
                double temp = bound[j];
                bound[j] = bound[j + 1];
                bound[j + 1] = temp;
            }
        }
    }
    int k = 0; double temp_len = len;
    for (int i = 0; i < 90; i++)
    {
        temp_len = temp_len * 2;
        k = i + 1;
        if (temp_len > bound[0])
        {
            break;
        }
    }
    ocn->loc_max[0] = ocn->loc_min[0] + temp_len; ocn->loc_max[1] = ocn->loc_min[1] + temp_len; ocn->loc_max[2] = ocn->loc_min[2] + temp_len;

    for (int i = 0; i < k; i++)
    {
        if(k>3)
        {
            oc_node_divide_all_leavesp(ocn);
        }
        else
        {
            oc_node_divide_all_leaves(ocn);
        }
    }
    return ocn;
}


static  void get_average_point_downsampling(OC_Node* ocn,Node**re)
{
    if(ocn==NULL)
    {
        return ;
    }
    if(ocn->children[0]==NULL)
    {
        if(ocn->value!=NULL)
        {
            double p[3] = { 0,0,0 };
            double sum = 0;

            for(Node* nit=(Node*)(ocn->value);nit!=NULL;nit=(Node*)(nit->Next))
            {
                template_v* v1=(template_v*)(nit->value);
                p[0] += v1->point[0]; p[1] += v1->point[1]; p[2] += v1->point[2]; 
                sum += 1;
            } 

            p[0] /= sum; p[1] /= sum; p[2] /= sum;    
            template_v* v = (template_v*)malloc(sizeof(template_v));
            Vertex_init_(v);
            v->point_size = 3;
            v->point = (double*)malloc(sizeof(double)*3);
            memmove(v->point, p, sizeof(double) * 3);
            (*re) = node_overlying(*re, v);
        }
    }
    else
    {
        for(int i=0;i<8;i++)
        {
            get_average_point_downsampling(ocn->children[i],re); 
        }
    }  
}
#include<thread>

Node* downsampling_through_size(Node* n,double len )
{
    OC_Node* ocn = get_oc_node_according_to_size(n, len);
    if(ocn==NULL)
    {
        return NULL;
    } 
    Node* re[8] ={0};
    std::thread mythread0(get_average_point_downsampling,ocn->children[0],&(re[0]));
    std::thread mythread1(get_average_point_downsampling,ocn->children[1],&(re[1]));
    std::thread mythread2(get_average_point_downsampling,ocn->children[2],&(re[2]));
    std::thread mythread3(get_average_point_downsampling,ocn->children[3],&(re[3]));
    std::thread mythread4(get_average_point_downsampling,ocn->children[4],&(re[4]));
    std::thread mythread5(get_average_point_downsampling,ocn->children[5],&(re[5]));
    std::thread mythread6(get_average_point_downsampling,ocn->children[6],&(re[6]));
    std::thread mythread7(get_average_point_downsampling,ocn->children[7],&(re[7]));
    mythread0.join();
    mythread1.join();
    mythread2.join();
    mythread3.join();
    mythread4.join();
    mythread5.join();
    mythread6.join();
    mythread7.join();
    Node* re1=re[0];
    for(int i=1;i<8;i++)
    { 
        Node* inv=node_reverse(re[i]);
        if(inv!=NULL)
        {
            inv->Next=(void*)re1;
            if(re1!=NULL)
            {
                re1->Prev=(void*)inv;
            }
            re1=re[i];
        }
    }
    oc_node_free(ocn);
    int i=0;
    for(Node* nit=re1;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        v->id=i;
        i++;
    }  

    return re1;
}


Node* downsampling_through_size1(Node* n,double len )
{
    double* box= mesh_compute_bounding_box_from_node(n);
    double bound[3]={box[3]-box[0],box[4]-box[1],box[5]-box[2]};
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 2; j++)
        {
            if (bound[j] < bound[j + 1])
            {
                double temp = bound[j];
                bound[j] = bound[j + 1];
                bound[j + 1] = temp;
            }
        }
    } 
    int size=(int)(bound[0]/len);
    size++;

    Node**** ns=(Node****)malloc(sizeof(Node***)*size );
    for(int i=0;i<size;i++)
    {
        ns[i]=(Node***)malloc(sizeof(Node**)*size);
        for(int j=0;j<size;j++)
        {
            ns[i][j]=(Node**)malloc(sizeof(Node*)*size);
            memset(ns[i][j],0,sizeof(Node*)*size);
        }
    } 
    for(Node*nit=n;nit!=NULL;nit=(Node*)(nit->Next) )
    {
        template_v* v=(template_v*)(nit->value);
        int i=(v->point[0]-box[0])/len;
        int j=(v->point[1]-box[1])/len;
        int k=(v->point[2]-box[2])/len;

        i>=size ? (i=size-1): (i=i);
        j>=size ? (j=size-1): (j=j);
        k>=size ? (k=size-1): (k=k);  
        ns[i][j][k]=node_overlying(ns[i][j][k],v);
    } 

    Node* re1=NULL;
    double sum=0;
    double p[3] = { 0,0,0 };
    for(int i=0;i<size;i++  )
    {
        for(int j=0;j<size;j++)
        {
            for(int k=0;k<size;k++)
            {
                if(ns[i][j][k]!=NULL)
                {
                    sum = 0;
                    memset(p,0,sizeof(double)*3);
                    for(Node* nit=ns[i][j][k];nit!=NULL;nit=(Node*)(nit->Next))
                    {
                        template_v* v1=(template_v*)(nit->value);
                        p[0] += v1->point[0]; p[1] += v1->point[1]; p[2] += v1->point[2]; 
                        sum += 1;
                    } 

                    p[0] /= sum; p[1] /= sum; p[2] /= sum;
                    template_v* v = (template_v*)malloc(sizeof(template_v));
                    Vertex_init_(v);
                    v->point_size = 3;
                    v->point = (double*)malloc(sizeof(double)*3);
                    memmove(v->point, p, sizeof(double) * 3);
                    re1 = node_overlying(re1, v);
                    free_node(ns[i][j][k]);
                } 
            }
            SAFE_FREE(ns[i][j]);
        }
        SAFE_FREE(ns[i]);
    }  
    SAFE_FREE(ns);
    free(box);  
    return re1;
}

Node* downsampling_through_oc_tree(Node* vertices,int depth)
{
    OC_Node* ocn=(OC_Node*)malloc(sizeof(OC_Node));
    oc_node_init_from_node(ocn,vertices);

    for(int i=0;i<depth;i++)
    {
        oc_node_divide_all_leaves(ocn); 
    }
    Node* re=NULL;
    // Mesh* re=(Mesh*)malloc(sizeof(Mesh));
    // Mesh_init(re);
    // int id=0;
    double p[3]={0};
    for(auto oit=octree_begin(ocn);oit.it!=NULL;oit++)
    {
        if(oit.it->value==NULL)
        {
            continue;
        }

        p[0]=(oit.it->loc_min[0]+oit.it->loc_max[0])/2.0;p[1]=(oit.it->loc_min[1]+oit.it->loc_max[1])/2.0;p[2]=(oit.it->loc_min[2]+oit.it->loc_max[2])/2.0;
        template_v* v = (template_v*)malloc(sizeof(template_v));
        Vertex_init_(v);
        v->point_size = 3;
        v->point = (double*)malloc(sizeof(double)*3);
        memmove(v->point, p, sizeof(double) * 3);
        
        re=node_overlying(re,v);

    }

    oc_node_free(ocn);
    return re;
}

static inline int my_cmp(void * a,void* b)
{
    Int_RB_Tree* tree1=(Int_RB_Tree*)a,*tree2=(Int_RB_Tree*)b;

    return (tree1->size>tree2->size); 
}

Node* get_cluster_points_from_mesh1(Mesh * m,double r)
{
    Int_RB_Tree* tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree); 
    int size=m->num_v(m);

    template_v**values=(template_v**)malloc(sizeof(template_v*)*size);
    int i=0;
    for(auto it=m->v_begin(m);it!=m->v_end(m);it++)
    {
        values[i]=quote(it); 
        i++;
    }
    KD_Node* kdtree=create_kd_tree(values,size,0);
    free(values);

    Node* re=NULL;
    for(auto it= m->v_begin(m);it!=m->v_end(m);it++)
    {
        if(tree->find(tree,quote(it)->id)==NULL)
        {
            Node* find_ts=NULL;
            Node* n= kd_tree_find_nearest_sphere(quote(it),r,kdtree );
            for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
            {
                template_v* v=(template_v*)(nit->value);
                Int_RB_Tree* tree1=(Int_RB_Tree*)(tree->find(tree,v->id));
                if( tree1!=NULL && node_find(find_ts,tree1)==NULL)
                {
                    find_ts=node_overlying(find_ts,tree1);
                }
            }
            if(find_ts==NULL)
            {
                Int_RB_Tree* tree1=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
                int_rb_tree_init(tree1);
                for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
                {
                    template_v* v=(template_v*)(nit->value);
                    tree->insert(tree,v->id,tree1);
                    tree1->insert(tree1,v->id,v); 
                } 
                re=node_overlying(re,tree1);
            }
            else
            {
                Node* nit=find_ts;
                Int_RB_Tree* tree1=(Int_RB_Tree*)(nit->value);
                nit=(Node*)(nit->Next); 

                for( ;nit!=NULL ; nit=(Node*)(nit->Next))
                {
                    Int_RB_Tree*tree2=(Int_RB_Tree*)(nit->value);
                    for(auto it1=tree2->begin(tree2);it1.it!=NULL;it1++)
                    {
                        template_v* v=(template_v*)(it1.second) ;
                        tree1->insert(tree1,v->id,v);
                        tree->insert(tree,v->id,tree1); 
                    }

                    re=node_delete_value(re,tree2);
                    int_rb_tree_free(tree2);
                }
                
                for(nit=n;nit!=NULL;nit=(Node*)(nit->Next))
                {
                    template_v* v=(template_v*)(nit->value);
                    
                    tree1->insert(tree1,v->id,v);
                    tree->insert(tree,v->id,tree1);
                }
            }
            free_node(find_ts);
            free_node(n);
        } 
    }
    free_kdnode(kdtree);
    int_rb_tree_free(tree);
    re=node_bub_sortn(re,my_cmp);
    return re;
}

double** compute_optimal_rigid_body_transformation(double** data1,double** data2,int len)
{
    double **re=(double**)malloc(sizeof(double*)*4);
    for(int i=0;i<4;i++)
    {
        re[i]=(double*)malloc(sizeof(double)*4);
        memset(re[i],0,sizeof(double)*4);
    }
    double ave1[3]={0},ave2[3]={0};
    
    for(int i=0;i<len;i++ )
    {
        for(int j=0;j<3;j++)
        {
            ave1[j]+=data1[i][j];
            ave2[j]+=data2[i][j];
        }
    }
    for(int i=0;i<3;i++)
    {
        ave1[i]/=(double)len;
        ave2[i]/=(double)len;
    }
    // printf("%lf %lf %lf\n",ave1[0],ave1[1],ave1[2]);
    // printf("%lf %lf %lf\n",ave2[0],ave2[1],ave2[2]);
 
    double **H=(double**)malloc(sizeof(double*)*3);
    for(int i=0;i<3;i++)
    {
        H[i]=(double*)malloc(sizeof(double)*3);
        memset(H[i],0,sizeof(double)*3);
    }
    for(int m=0;m<len;m++)
    {
        for(int i=0;i<3;i++)
        {
            for(int j=0;j<3;j++)
            {
                H[i][j]+=(data2[m][i]-ave2[i])*(data1[m][j]-ave1[j]);
            }
        }
    } 
    double s[3]={0};

    double **VT=(double**)malloc(sizeof(double*)*3);
    for(int i=0;i<3;i++)
    {
        VT[i]=(double*)malloc(sizeof(double)*3);
        memset(VT[i],0,sizeof(double)*3);
    }

    // printf("H:\n");
    // for(int i=0;i<3;i++)
    // {
    //     for(int j=0;j<3;j++)
    //     {
    //          printf("%lf ",H[i][j]);
    //     }
    //     printf("\n");
    // }


    dsvd(H,3,3,s,VT);

    // printf("s: %lf %lf %lf\n",s[0],s[1],s[2]);

    // printf("VT:\n");
    // for(int i=0;i<3;i++)
    // {
    //     for(int j=0;j<3;j++)
    //     {
    //          printf("%lf ",VT[i][j]);
    //     }
    //     printf("\n");
    // }


    double temp[9]={0};
    memset(temp,0,sizeof(double)*9);

    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            for(int k=0;k<3;k++)
            {
                temp[i*3+j]+=(H[i][k]*VT[k][j]);
            }
        }
    }

    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            temp[i*3+j]/=s[j];
        }
    } 


    
    double temp1[9]={0};
    memset(temp1,0,sizeof(double)*9);

    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            for(int k=0;k<3;k++)
            {
                temp1[i*3+j]+=(temp[i*3+k]*VT[j][k]  );
            }
        }
    }

    // printf("temp:\n");
    // for(int i=0;i<3;i++)
    // {
    //     for(int j=0;j<3;j++)
    //     {
    //          printf("%lf ",temp1[i*3+j]);
    //     }
    //     printf("\n");
    // }

 
    for(int i=0;i<3;i++)
    {
        re[i][3]=0;
        for(int k=0;k<3;k++)
        {
            re[i][3]+=temp1[i*3+k]*ave1[k];
        }
        re[i][3]=ave2[i]-re[i][3];
    }


    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            re[i][j]=temp1[i*3+j]; 
        }
    }
 
    re[3][3]=1.0;

    for(int i=0;i<3;i++)
    {
        free(H[i]);
        free(VT[i]);
    } 
    free(H);
    free(VT);

    return re;
}

void points_cloud_compute_normalr(Node* n,KD_Node*kdtree,double r)
{
    int i=0;
    double* normal=NULL;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        Node* n1=kd_tree_find_nearest_sphere(v,r,kdtree);
        double**axis=compute_pca_result(n1);
        normal=(double*)malloc(sizeof(double)*3);
        v->prop=normal;
        if(axis==NULL)
        {
            normal[0]=0;normal[1]=0;normal[2]=1.0;
        }
        else
        {
            normal[0]=axis[0][0];normal[1]=axis[1][0];normal[2]=axis[2][0];
            for(int j=0;j<v->point_size;j++)
            {
                free(axis[j]);
            }
            free(axis);
        }
        free_node(n1);
        i++;
    } 


}
// void points_cloud_compute_correct_normals_with_eigen(Node* n)
// {
// }

